#!/usr/bin/python3
# -*- coding: utf-8 -*-
# 电机驱动 L98n 基本运动方法
import RPi.GPIO as GPIO
import time
import _thread

GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

#右轮（默认低电平）
IN1 = 22
IN2 = 23
pwm_right = 24
#左轮（默认高电平）
IN3 = 27
IN4 = 18
pwm_left = 17
#状态值（前进、后退、静止）
ZT_FORWARD = 1
ZT_BACK = -1
ZT_STATIC = 0
#默认值：占空比
DEFAULT_ON_OFF_RATIO = 100

#指令池
InstructMap = None
#运动指令名称
INSTRUCT_KEY = "MoveInstruct"
#检测指令的间隔时间（单位：秒）
CHECK_INSTRUCT_TIMESPACE = 0.1
#运动指令值编号
#继续保持当前运动状态
MOVE_INSTRUCT_GOON = 0
#停止
MOVE_INSTRUCT_STOP = 1

#声明pwm
p_left = None
p_right = None

#基础四轮运动：
#---静止-------------
#静止
#---匀速运动---------
#前进
#后退
#静止状态向右转【极少使用，除非判断空间极小】
#静止状态向左转【极少使用，除非判断空间极小】
#运动向右转
#运动向左转
#静止后退右转【极少使用，除非判断空间极小】
#静止后退左转【极少使用，除非判断空间极小】
#运动后退右转
#运动后退左转
#匀速原地向右转
#匀速原地向左转
#---变速运动---------
#加速前进
#加速后退
#静止状态向右转【极少使用，除非判断空间极小】
#静止状态向左转【极少使用，除非判断空间极小】
#运动向右转
#运动向左转
#静止后退右转【极少使用，除非判断空间极小】
#静止后退左转【极少使用，除非判断空间极小】
#运动后退右转
#运动后退左转
#加速原地向右转
#加速原地向左转

#总结：
#左右两个轮子的状态都是：静止、匀速向前、加速向前、【减速向前】、匀速向后、加速向后【减速向前】。5种状态，组合后一共25种状态

def init(instructMap = {INSTRUCT_KEY:MOVE_INSTRUCT_GOON}):
    GPIO.setup(IN1, GPIO.OUT)
    GPIO.setup(IN2, GPIO.OUT)
    GPIO.setup(IN3, GPIO.OUT)
    GPIO.setup(IN4, GPIO.OUT)
    GPIO.setup(pwm_left, GPIO.OUT)
    GPIO.setup(pwm_right, GPIO.OUT)
    global p_left, p_right
    p_left = GPIO.PWM(pwm_left,80)
    p_left.start(0)
    p_right = GPIO.PWM(pwm_right,80)
    p_right.start(0)
    global InstructMap
    InstructMap = instructMap


#左轮(状态， 频率)  状态： 0 静止  1 向前  -1 向后   占空比：转速
def leftWheel(zt, hz=DEFAULT_ON_OFF_RATIO):
    p_left.ChangeDutyCycle(hz)
    if ZT_FORWARD == zt:
        GPIO.output(IN3, GPIO.HIGH)
        GPIO.output(IN4, GPIO.LOW)
    elif ZT_BACK == zt:
        GPIO.output(IN3, GPIO.LOW)
        GPIO.output(IN4, GPIO.HIGH)
    else:
        GPIO.output(IN3, False)
        GPIO.output(IN4, False)

#右轮(状态， 频率)  状态： 0 静止  1 向前  -1 向后   占空比：转速
def rightWheel(zt, hz=DEFAULT_ON_OFF_RATIO):
    p_right.ChangeDutyCycle(hz)
    if ZT_FORWARD == zt:
        GPIO.output(IN1, GPIO.HIGH)
        GPIO.output(IN2, GPIO.LOW)
    elif ZT_BACK == zt:
        GPIO.output(IN1, GPIO.LOW)
        GPIO.output(IN2, GPIO.HIGH)
    else:
        GPIO.output(IN1, False)
        GPIO.output(IN2, False)
       
#检测指令
def checkInstruct(times):
    for i in range(int(times/CHECK_INSTRUCT_TIMESPACE)):
        if MOVE_INSTRUCT_STOP == InstructMap[INSTRUCT_KEY]:
            break
        else:
            time.sleep(CHECK_INSTRUCT_TIMESPACE)
       
#1 前进
def forward(times):
    print("前进")
    leftWheel(ZT_FORWARD)
    rightWheel(ZT_FORWARD)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
    
#2 向右转
def turnRight(times):
    print("向右转")
    leftWheel(ZT_FORWARD)
    rightWheel(ZT_STATIC)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)

#3 向左转
def turnLeft(times):
    print("向左转")
    leftWheel(ZT_STATIC)
    rightWheel(ZT_FORWARD)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)

#4 后退
def back(times):
    print("后退")
    leftWheel(ZT_BACK)
    rightWheel(ZT_BACK)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
 
#5 右后退
def backRight(times):
    print("右后退")
    leftWheel(ZT_BACK)
    rightWheel(ZT_STATIC)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
    
#6 左后退
def backLeft(times):
    print("左后退")
    leftWheel(ZT_STATIC)
    rightWheel(ZT_BACK)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
    
#7 顺时针转圈(向右转圈)
def aroundCW(times):
    print("顺时针转圈(向右转圈)")
    leftWheel(ZT_FORWARD)
    rightWheel(ZT_BACK)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)

#8 逆时针转圈(向左转圈)
def aroundCCW(times):
    print("逆时针转圈(向左转圈)")
    leftWheel(ZT_BACK)
    rightWheel(ZT_FORWARD)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
    
#9 静止
def noMove(times):
    print("静止")
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)
    checkInstruct(times)
    leftWheel(ZT_STATIC)
    rightWheel(ZT_STATIC)

#资源清理
def clean():
    p_left.stop()
    p_right.stop()
    GPIO.cleanup()
    
#-------------TEST----------------

if __name__ == "__main__":
    testInstructMap = {INSTRUCT_KEY:MOVE_INSTRUCT_GOON}
    init(testInstructMap)
    try:
        #forward(3)#大约2m
        #turnRight(0.75)
        #turnLeft(0.75)
        #backRight(0.75)
        #backLeft(0.75)
        #aroundCW(0.8)
        aroundCCW(0.8)
    except KeyboardInterrupt:
        clean()
        print("捕捉异常后清理资源完成")
    else:
        clean()
        print("正常测试结束")
